using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.DirectX;

namespace Noein.Physics
{
    public class Constants
    {
        /// <summary>
        /// acceleration due to gravity on earth surface
        /// </summary>
        public const float g = 4f;//9.8f;

        /// <summary>
        /// Gravitational constant
        /// </summary>
        public const double G = 0.0000000000667;

    }


    public class Functions
    {
        /// <summary>
        /// force on point mass 2 due to point mass 1 according to Newton's Law of Universal Gravitation
        /// </summary>
        public static Vector3 Gravitation(Object object1, Object object2)
        {
            // distance between masses
            Vector3 direction = object2.Spatial.Position - object1.Spatial.Position;
            float distanceSq = direction.LengthSq();

            try
            {
                float scalar = (float)((object1.Physical.Mass * object2.Physical.Mass) / distanceSq * Constants.G);
                direction.Normalize();
                return scalar * direction;
            }
            catch (DivideByZeroException)
            {
                // TODO two masses overlap, what to do
                return Vector3.Empty;
            }
        }

        #region TODO
        public static Vector3 StaticFriction()
        {
            return new Vector3();
        }

        public static Vector3 KineticFriction()
        {
            return new Vector3();
        }

        public static Vector3 Spring()
        {
            // Hooke's Law: F = -kx, k = spring constant (larger-strong, small-loose)
            // damping: F = k_d * dx/dt
            return new Vector3();
        }
        // TODO turbulence

        // TODO force field

        // TODO impulse
        #endregion

        /// <summary>
        /// Assuming object 1 and 2 collide, apply impulse to both of them.  Impulse is a property of a collision,
        /// not necessarily an object.
        /// </summary>
        /// <param name="object1"></param>
        /// <param name="object2"></param>
        /// <param name="lineOfAction">normalized line perpendicular to the colliding surface</param>
        /// <param name="cor">1 = perfectly elastic, 0 = perfectly inelastic, >1 = collision generates kinetic energy (explosion), <0 = attraction</param>
        public static void ApplyImpulse(Object object1, Object object2, Vector3 lineOfAction, float cor)//Physics.PhysicsInfo object1, Physics.PhysicsInfo object2, Vector3 lineOfAction, float cor)
        {
            // rotational component in the divisor of impulse force calculation

            // n dot [I_1^-1 (r1 cross n)] cross r1, n = unit vector normal to surface of contact
            //Vector3 worldR1 = Vector3.TransformCoordinate(object1.PhysicsInfo.RelativePosition, object1.Spatial.OrientationMatrix);
            //Vector3 v1 = Vector3.Cross(worldR1, lineOfAction);
            //Matrix inverseInertia1 = object1.PhysicsInfo.InverseWorldMomentOfInertia(object1.Spatial.OrientationMatrix);
            //v1.TransformCoordinate(inverseInertia1);
            //v1 = Vector3.Cross(v1, worldR1);
            //float f1 = Vector3.Dot(lineOfAction, v1);

            // n dot [I_2^-1 (r2 cross n)] cross r2
            //Vector3 worldR2 = Vector3.TransformCoordinate(object2.PhysicsInfo.RelativePosition, object2.Spatial.OrientationMatrix);
            //Vector3 v2 = Vector3.Cross(worldR2, lineOfAction);
            //Matrix inverseInertia2 = object2.PhysicsInfo.InverseWorldMomentOfInertia(object2.Spatial.OrientationMatrix);
            //v2.TransformCoordinate(inverseInertia2);
            //v2 = Vector3.Cross(v2, worldR2);
            //float f2 = Vector3.Dot(lineOfAction, v2);

            // scalar velocities along the line of action
            //float speed1 = System.Math.Abs(Vector3.Dot(object1.PhysicsInfo.Velocity, lineOfAction));
            //float speed2 = System.Math.Abs(Vector3.Dot(object2.PhysicsInfo.Velocity, lineOfAction));
            float relativeSpeed =
                Vector3.Dot((object1.Physical.Velocity - object2.Physical.Velocity), lineOfAction);

            // J = [ -1(v_1 - v_2)_before (e + 1) ] / [ 1/m_1 + 1/m_2 + f1 + f2 ], e = cor
            float impulse = -relativeSpeed * (cor + 1) /
                (object1.Physical.OneOverMass + object2.Physical.OneOverMass);// + f1 + f2);
            Vector3 impulseV = impulse * lineOfAction;

            // TODO flip += and -= if direction is off
            // v_after = v_before + J n / mass
            object1.Physical.Momentum += impulseV;
            object2.Physical.Momentum -= impulseV;

            // omega_after = omega_before + I^-1 (r cross (Jn))
            //object1.PhysicsInfo.AngularVelocity +=
            //    Vector3.TransformCoordinate(Vector3.Cross(worldR1, impulseV), inverseInertia1);
            //object2.PhysicsInfo.AngularVelocity +=
            //    Vector3.TransformCoordinate(Vector3.Cross(worldR2, impulseV), inverseInertia2);
        }
        
        //public static float[] MassMomentsOfInertia(Geometry shape, float mass, params float[] dimensions)
        //{
        //    float[] retVal = new float[3];

        //    float r2 = 0;
        //    //switch (shape)
        //    //{
        //    //    case Geometry.CircularCylinder:
        //    //        // I_xx = I_yy = (1/4) m r^2 + (1/12) m l^2; I_zz = (1/2) m r^2
        //    //        r2 = dimensions[0] * dimensions[0];
        //    //        retVal[0] = retVal[1] =
        //    //            0.25f * mass * r2 + 0.0833333f * mass * dimensions[1] * dimensions[1];
        //    //        retVal[3] = 0.5f * mass * r2;
        //    //        break;

        //    //    case Geometry.CircularCylindricalShell:
        //    //        r2 = dimensions[0] * dimensions[0];
        //    //        retVal[0] = retVal[1] = 0.5f * mass * r2 + 0.0833333f * mass * dimensions[1] * dimensions[1];
        //    //        retVal[2] = mass * r2;
        //    //        break;

        //    //    case Geometry.RectangularCylinder:
        //    //        float a2 = dimensions[1] * dimensions[1];   // y axis
        //    //        float l2 = dimensions[2] * dimensions[2];   // z axis
        //    //        float b2 = dimensions[0] * dimensions[0];   // x axis
        //    //        float mOver12 = 0.0833333f * mass;
        //    //        retVal[0] = mOver12 * (a2 + l2);
        //    //        retVal[1] = mOver12 * (b2 + l2);
        //    //        retVal[2] = mOver12 * (a2 + b2);
        //    //        break;

        //    //    case Geometry.Sphere:
        //    //        retVal[0] = retVal[1] = retVal[2] = 0.4f * mass * dimensions[0] * dimensions[0];
        //    //        break;

        //    //    case Geometry.SphericalShell:
        //    //        retVal[0] = retVal[1] = retVal[2] = 0.6666667f * mass * dimensions[0] * dimensions[0];
        //    //        break;
        //    //}

        //    return retVal;
        //}
    }

    #region forces
    public abstract class Force
    {
        public abstract Vector3 Compute(Object obj);
    }

    public class SpringForce : Force
    {
        public Object obj1, obj2;
        public float r;
        public float ks, kd;

        public SpringForce(Object obj1, Object obj2, float springConstant, float damperConstant, float restLength)
        {
            this.obj1 = obj1;
            this.obj2 = obj2;

            ks = springConstant;
            kd = damperConstant;
            r = restLength;
        }

        public override Vector3 Compute(Object obj)
        {
            if (obj != obj1 && obj != obj2)
                return Vector3.Empty;
            
            // compute direction
            Vector3 springForce = obj2.Spatial.Position - obj1.Spatial.Position;
            springForce.Normalize();

            // compute scalar
            float scalar = (obj1.Spatial.Position - obj2.Spatial.Position).Length();
            // add spring component
            scalar = (scalar - r) * ks;

            // add damper component
            float damp = Vector3.Dot(obj1.Physical.Velocity - obj2.Physical.Velocity, springForce) * kd;

            scalar -= damp;

            //scalar -= damp < scalar ? damp : scalar;

            springForce.Multiply(scalar);

            if (obj == obj2)
                springForce.Multiply(-1f);

            return springForce;
        }
    }

    public class MuscleForce : Force
    {
        public Object obj1, obj2;

        public float kd;

        private float scalar;
        public float Scalar
        {
            get { return scalar; }
            set { scalar = value; }
        }

        public MuscleForce(Object obj1, Object obj2, float damperConstant)
        {
            this.obj1 = obj1;
            this.obj2 = obj2;
            this.kd = damperConstant;
        }

        public override Vector3 Compute(Object obj)
        {
            if (obj != obj1 && obj != obj2)
                return Vector3.Empty;

            // compute direction
            Vector3 muscleForce = obj2.Spatial.Position - obj1.Spatial.Position;
            muscleForce.Normalize();

            float s = scalar;
            s -= Vector3.Dot(obj1.Physical.Velocity - obj2.Physical.Velocity, muscleForce) * kd;

            // HACK cap output
            if (s < -5)
                s = -5;
            else if (s > 5)
                s = 5;

            muscleForce.Multiply(s);

            if (obj == obj2)
                muscleForce.Multiply(-1f);

            return muscleForce;
        }
    }

    public class EarthSurfaceGravity : Force
    {
        private static EarthSurfaceGravity instance;

        private EarthSurfaceGravity() { }
        public static EarthSurfaceGravity Instance()
        {
            if (instance == null)
                instance = new EarthSurfaceGravity();

            return instance;
        }

        public override Vector3 Compute(Object obj)
        {
            return obj.Physical.Mass * Constants.g * new Vector3(0, -1f, 0);
        }
    }

    public class ConstantForce : Force
    {
        private Vector3 force;

        public ConstantForce(Vector3 force)
        {
            this.force = force;
        }

        public override Vector3 Compute(Object obj)
        {
            return force;
        }
    }

    public class Friction : Force
    {
        private float muS;
        public float CoefOfStaticFriction
        {
            get { return muS; }
            set { muS = value; }
        }

        private float muK;
        public float CoefOfKineticFriction
        {
            get { return muK; }
            set { muK = value; }
        }

        public Friction(float muS, float muK)
        {
            this.muS = muS;
            this.muK = muK;
        }

        public override Vector3 Compute(Object obj)
        {
            Vector3 contact = obj.Physical.contact;
            if (contact == Vector3.Empty)
                return Vector3.Empty;

            //// TODO contact check, right now assuming in contact
            if (obj.Physical.Velocity == Vector3.Empty)
            {
                // static friction, against other forces
                if (obj.Physical.Force != Vector3.Empty)
                {
                    float maxFriction = obj.Physical.Mass * Physics.Constants.g * muS;

                    // separate total force into two component, 1 unaffected by friction, 2 affected
                    Vector3 force1 = Vector3.Multiply(contact, Vector3.Dot(contact, obj.Physical.Force));
                    Vector3 force2 = obj.Physical.Force - force1;

                    // HACK assume force always act parallel to the contact surface
                    if (force2.LengthSq() < maxFriction * maxFriction)
                    {
                        // total force less than friction, complete cancellation
                        return force2 * -1;
                    }
                    else
                    {
                        // total force exceeds friction
                        return (force2 - maxFriction * Vector3.Normalize(force2));//Vector3.Normalize(obj.Physical.Force) * -1 * maxFriction;
                    }
                }

                return Vector3.Empty;
            }
            else
            {
                // sparate velocity into two component
                Vector3 v1 = Vector3.Multiply(contact, Vector3.Dot(contact, obj.Physical.Velocity));
                Vector3 v2 = obj.Physical.Velocity - v1;

                // damping
                if (v2.LengthSq() < 0.001f)
                {
                    obj.Physical.Momentum = v1 * obj.Physical.Mass;
                    return Vector3.Empty;
                }

                // kinetic friction, against velocity
                return Vector3.Normalize(v2) * -1 * obj.Physical.Mass * Physics.Constants.g * muK;
                //Vector3.Normalize(obj.Physical.Velocity) * -1 * obj.Physical.Mass * Physics.Constants.g * muK;
            }
        }
    }

    //public class Friction : Force
    //{
    //    public override void Apply(List<Object> targets)
    //    {
    //        //Vector3 r = new Vector3(0, -target.PhysicsInfo.Dimensions[0], 0);
    //        //Vector3 forwardDirection = Vector3.Normalize(target.PhysicsInfo.Velocity);
    //        //float forwardSpeed = target.PhysicsInfo.Velocity.Length();
    //        //float backwardSpeed = Vector3.Dot(Vector3.Cross(target.PhysicsInfo.AngularVelocity, r), forwardDirection);

    //        //Vector3 friction;
    //        //if (forwardSpeed < 0.001f)
    //        //{
    //        //    // zero out linear and rotational motion due to friction when object is not moving
    //        //    friction = Vector3.Empty;
    //        //    target.PhysicsInfo.Momentum = Vector3.Empty;
    //        //    target.PhysicsInfo.AngularMomentum = Vector3.Empty;
    //        //}
    //        //else if ((forwardSpeed - backwardSpeed) < 0.001f)
    //        //{
    //        //    // rolling
    //        //    friction = ur * target.PhysicsInfo.Mass * Physics.Constants.g * (-1f) * forwardDirection;
    //        //}
    //        //else
    //        //{
    //        //    // sliding
    //        //    friction = us * target.PhysicsInfo.Mass * Physics.Constants.g * (-1f) * forwardDirection;
    //        //}

    //        // Torque
    //        //target.PhysicsInfo.Torque = Vector3.Cross(
    //        //new Vector3(0f, -target.PhysicsInfo.Dimensions[0], 0f), friction);
    //    }
    //}


    #endregion

}
